#include "Signal.h"
#include "MPC_EW1D_PatternedW_b.h"

const double k_w = 3.3829e-12;
const double a_w = 0.6042;
const double E_w = 2.7e-3; // Unit = EV
const double k_c = 1.0274e-13;
const double a_c = 0.1669;
const double E_c = 1.9e-3; // Unit = EV
const double a_v = 15.55493;
const double k_v = 20.64504;
const double a_t = 0.02332;
const double k_t = 0.0261;
const double k_B = 8.617343e-5; // Boltzmann's constant, Unit = ev/K

/*const double a_c2=23.9948;
const double b_c2=26.4299;
const double c_c2=-32.58;
const double d_c2=8.2319;
const double e_c2=-0.112;
const double a_sigma2=97.0163;
const double b_sigma2=-66.2908;
const double c_sigma2=8.0974;
const double d_sigma2=1.916;
const double e_sigma2=-0.0296;*/

// -----------------------------------------------------------

// C2 = C2(W)
//eq_C2::eq_C2(int i_LatticeSize):MathFunction(1,5),LatticeSize(i_LatticeSize){}
//
//eq_C2::~eq_C2(){}
//
//double eq_C2::value(const double *x){
//  double W = x[0];
//  return exp(a_c2*pow(W,4)+b_c2*pow(W,3)+c_c2*pow(W,2)+d_c2*pow(W,1)+e_c2);// parameter fitting to rough
//  //return a_c2*pow(W,4)+b_c2*pow(W,3)+c_c2*pow(W,2)+d_c2*pow(W,1)+e_c2;
//}
//
//void eq_C2::gradient(const double *x,double *grad){
//  double W = x[0];
//
//  grad[0] = (4*a_c2*pow(W,3)+3*b_c2*pow(W,2)+2*c_c2*pow(W,1)+d_c2)*exp(a_c2*pow(W,4)+b_c2*pow(W,3)+c_c2*pow(W,2)+d_c2*pow(W,1)+e_c2);// parameter fitting to rough
//	//grad[0] = (4*a_c2*pow(W,3)+3*b_c2*pow(W,2)+2*c_c2*pow(W,1)+d_c2);
//}
//
//bool eq_C2::parameterize(const double *para){
//  LatticeSize = int(para[0]);
//  return true;
//}
//
//bool eq_C2::parameterize(int para_ID,double para_val){
//  switch(para_ID){
//  case 0:
//    LatticeSize = int(para_val);break;
//  default:
//    cerr << "eq_C2::parameterize(): invalid PARA_ID\n";
//  }
//  return true;
//}
//// ------------------------------------------------------------
//
//eq_sigma2::eq_sigma2(int i_LatticeSize):MathFunction(1,2),LatticeSize(i_LatticeSize)
//{}
//
//
//eq_sigma2::~eq_sigma2(){}
//
//double eq_sigma2::value(const double *x){
//  double W = x[0];
//  return a_sigma2*W+b_sigma2; //parameter fitting to rough
//  //return a_sigma2*pow(W,4)+b_sigma2*pow(W,3)+c_sigma2*pow(W,2)+d_sigma2*pow(W,1)+e_sigma2;
//}
//
//void eq_sigma2::gradient(const double *x,double *grad){
//  double W = x[0];
//
//  grad[0]= a_sigma2;  //parameter fitting to rough
// //grad[0] = (4*a_sigma2*pow(W,3)+3*b_sigma2*pow(W,2)+2*c_sigma2*pow(W,1)+d_sigma2);
//}
//
//bool eq_sigma2::parameterize(const double *para){
//  LatticeSize = int(para[0]);
//  return true;
//}
//
//bool eq_sigma2::parameterize(int para_ID,double para_val){
//  switch(para_ID){
//  case 0:
//    LatticeSize = int(para_val);
//    break;
//  default:
//    cerr << "eq_sigma2::parameterize(): invalid PARA_ID\n";
//  }
//  return true;
//}
// --------------------------------------------------------------
eq_cov_z_p::eq_cov_z_p(int i_p,int i_m,
		       double i_Xmax,int i_LatticeSize,
		       double i_dt, double i_cov_z_m):
MathFunction(1,5),p(i_p),m(i_m),
LatticeSize(i_LatticeSize),Xmax(i_Xmax),
dt(i_dt),cov_z_m(0),
f_c2(i_LatticeSize),
f_sigma2(i_LatticeSize)
{}

eq_cov_z_p::~eq_cov_z_p(){}

double eq_cov_z_p::value(const double *x)
{
  double W = x[0];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);

  double lambda = -4.0*C2*(m*m)*pi*pi/(Xmax*Xmax);
  double temp1 = exp(2*lambda*dt);
  double temp2 = sigma2*(temp1-1)/(2*lambda);
  double cov_z_p = cov_z_m*temp1+temp2;
  return cov_z_p;
}

void eq_cov_z_p::gradient(const double *x,double *grad)
{
  double W = x[0];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);

  double grad_C2[1];
  double grad_sigma2[1];
  f_c2.gradient(x,grad_C2);
  f_sigma2.gradient(x,grad_sigma2);

  double lambda = -4.0*C2*(m*m)*pi*pi/(Xmax*Xmax);
  double temp1 = exp(2*lambda*dt);
  double dcov_z_dlambda =  2*temp1*cov_z_m*dt
    +sigma2*(4*lambda*temp1*dt-2*(temp1-1))/(4*lambda*lambda);
  double dlambda_dC2 = 4*pi*pi*(m*m)/(Xmax*Xmax);
  double dcov_z_dC2 = dcov_z_dlambda*dlambda_dC2;
  double dcov_z_dsigma2 = (temp1-1)/(2*lambda);

  grad[0]= grad_C2[0]*dcov_z_dC2+grad_sigma2[0]*dcov_z_dsigma2;

#ifdef DEBUG
  cout << "grad_C2[0]:" << grad_C2[0] << "\n";
  cout << "grad_C2[1]:" << grad_C2[1] << "\n";
  cout << "grad_sigma2[0]:" << grad_sigma2[0] << "\n";
  cout << "grad_sigma2[1]:" << grad_sigma2[1] << "\n";
  cout << "dcov_z_dC2:" << dcov_z_dC2 << "\n";
  cout << "dcov_z_dsigma2:" << dcov_z_dsigma2 << "\n";
#endif
}

bool eq_cov_z_p::parameterize(int para_ID,double para_val)
{
  switch(para_ID)
  {
  case 0:
    p = int(para_val);break;
  case 1:
    m = int(para_val);break;
  case 2:
    dt = para_val;break;
  case 3:
    cov_z_m = para_val;break;
  case 4:
    Xmax = para_val;break;
  case 5:
    LatticeSize =int(para_val);break;
    f_c2.parameterize(0,LatticeSize);
    f_sigma2.parameterize(0,LatticeSize);
  default:
    cerr << "eq_cov_z_p::parameterize(): invalid PARA_ID\n";
    exit(-1);
  }
  return true;
}
// -------------------------------------------------------------------
eq_z2_p::eq_z2_p(int i_p,int i_m,
		 double i_dt,double i_cov_z_m,double i_mean_z_m,
		 double i_Freq,double i_Xmax,int i_LatticeSize):
MathFunction(2,8),p(i_p),m(i_m),
dt(i_dt),cov_z_m(i_cov_z_m),mean_z_m(i_mean_z_m),
Freq(i_Freq),Xmax(i_Xmax),LatticeSize(i_LatticeSize),
f_c2(i_LatticeSize),f_sigma2(i_LatticeSize)
{}

eq_z2_p::~eq_z2_p(){}

double eq_z2_p::value(const double *x){
  double W = x[0];
  double A = x[1];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);
  double w_pm = eq_W_pm(W,A);

  double lambda = -4.0*C2*(m*m)*pi*pi/(Xmax*Xmax);
  double temp1 = exp(2*lambda*dt);
  double temp2 = sigma2*(temp1-1)/(2*lambda);
  double temp3 = exp(lambda*dt);
  double temp4 = (temp3-1)/lambda;

  double cov_z_p = cov_z_m*temp1+temp2;
  double mean_z_p = mean_z_m*temp3+w_pm*(temp3-1)/lambda;
  return cov_z_p+mean_z_p*mean_z_p;
}

void eq_z2_p::gradient(const double *x,double *grad){
  double W = x[0];
  double A = x[1];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);
  double w_pm = eq_W_pm(W,A);

  double lambda = -4.0*C2*(m*m)*pi*pi/(Xmax*Xmax);
  double temp1 = exp(2*lambda*dt);
  double temp2 = sigma2*(temp1-1)/(2*lambda);
  double temp3 = exp(lambda*dt);
  double temp4 = (temp3-1)/lambda;
  double mean_z_p = mean_z_m*temp3+w_pm*(temp3-1)/lambda;

  double grad_C2[1];
  double grad_sigma2[1];
  f_c2.gradient(x,grad_C2);
  f_sigma2.gradient(x,grad_sigma2);
  double dw_pm_dW;
  if(p==1&&m==0){
    dw_pm_dW = sqrt(Xmax);
  }
  else{
    dw_pm_dW = 0.0;
  }
  double dw_pm_dA = eq_dW_pm_dA();

  double dlambda_dW = -4.0*pi*pi*(m*m)/(Xmax*Xmax)*grad_C2[0];

  double dcov_z_dlambda =  2*temp1*cov_z_m*dt
    +sigma2*(4*lambda*temp1*dt-2*(temp1-1))/(4*lambda*lambda);
  double dlambda_dC2 = -4*pi*pi*(m*m)/(Xmax*Xmax);
  double dcov_z_dC2 = dcov_z_dlambda*dlambda_dC2;
  double dcov_z_dsigma2 = (temp1-1)/(2*lambda);

  double dcov_z_dW = dcov_z_dC2*grad_C2[0]+dcov_z_dsigma2*grad_sigma2[0];
  double dcov_z_dA = 0.0;  

  double dmean_z_dlambda = temp3*mean_z_m*dt
    +w_pm*(lambda*temp3*dt-(temp3-1))/(lambda*lambda);
  double dmean_z_dw_pm  = (temp3-1)/lambda;
  double dmean_z_dW = dmean_z_dlambda*dlambda_dW+dmean_z_dw_pm*dw_pm_dW;
  double dmean_z_dA = dmean_z_dw_pm*dw_pm_dA;

  grad[0]= dcov_z_dW+2*mean_z_p*dmean_z_dW;
  grad[1]= dcov_z_dA+2*mean_z_p*dmean_z_dA;

#ifdef DEBUG
  cout << "********************************************\n";
  cout << "[p,m,n] = [" << p << ", "  << m << ", " << n << "]\n";
  cout << "grad_C2[0]    :" << grad_C2[0] << "\n";
  cout << "grad_C2[1]    :" << grad_C2[1] << "\n";
  cout << "grad_sigma2[0]:" << grad_sigma2[0] << "\n";
  cout << "grad_sigma2[1]:" << grad_sigma2[1] << "\n";
  cout << "dcov_z_dC2    :" << dcov_z_dC2 << "\n";
  cout << "dcov_z_dsigma2:" << dcov_z_dsigma2 << "\n";
#endif
}

bool eq_z2_p::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    p = int(para_val);break;
  case 1:
    m = int(para_val);break;
  case 2:
    dt = para_val;break;
  case 3:
    cov_z_m = para_val;break;
  case 4:
    mean_z_m = para_val;break;
  case 5:
    Freq = para_val;break;
  case 6:
    Xmax = para_val;break;
  case 7:
    LatticeSize = int(para_val);
    f_c2.parameterize(0,LatticeSize);
    f_sigma2.parameterize(0,LatticeSize);
    break;
  default:
    cerr << "eq_z2_p::parameterize(): invalid para_ID\n";
  }
  return true;
}

bool eq_z2_p::parameterize(const double *para){
  p = int(para[0]);
  m = int(para[1]);
  dt = para[2];
  cov_z_m = para[3];
  mean_z_m = para[4];
  Freq = para[5];
  Xmax = para[6];
  LatticeSize = int(para[7]);

  return true;
}

double eq_z2_p::eq_W_pm(double W,double A){
  double w_pm;
  switch(p)
  {
  case 0:
    if(m==Freq)
    {
      w_pm=A*sqrt(Xmax/2);
    }
    else
    {
      w_pm=0.0;
    }
    break;
  case 1:
    if(m==0)
    {
      w_pm = W*sqrt(Xmax);
    }
    else
    {
      w_pm =0;
    }
    break;

  }
  return w_pm;
}

double eq_z2_p::eq_dW_pm_dA(){
  double dW_dA;
  switch(p)
  {
  case 0:
    if(m==Freq)
    {
      dW_dA=sqrt(Xmax/2);
    }
    else
    {
      dW_dA=0;
    }
    break;
  case 1:
    dW_dA=0;
    break;
  }
  return dW_dA;
}  


// ---------------------------------------------------------------
eq_r2_f::eq_r2_f(double i_dt,int i_mode,double i_Xmax, 
		 int i_LatticeSize,double i_Freq):
MathFunction(2,5),
dt(i_dt),mode(i_mode),Xmax(i_Xmax),
LatticeSize(i_LatticeSize),Freq(i_Freq),
z2_p(0,0,i_dt,0,0,i_Freq,i_Xmax,i_LatticeSize)
{
  int i;
  int dim = (mode+1);
  for(i=0;i<2;i++){
    cov_z_m[i] = NULL;
    mean_z_m[i] = NULL;
  }
}

eq_r2_f::~eq_r2_f(){cout << "~eq_r2_f()\n";}

double eq_r2_f::value(const double *x){
  int    p,m;
  double r2_f = 0.0;
  int    index;
  double para_z2_p[8];

  para_z2_p[2] = dt;
  para_z2_p[5] = Freq;
  para_z2_p[6] = Xmax;
  para_z2_p[7] = LatticeSize;
  for(m=0;m<=mode;m++)
  {
    if(m!=0)
    {
      index =m;
      for(p=0;p<2;p++)
      {
	para_z2_p[0] = p;
	para_z2_p[1] = m;
	para_z2_p[3] = cov_z_m[p][index];
	para_z2_p[4] = mean_z_m[p][index];
	z2_p.parameterize(para_z2_p);
	r2_f += z2_p.value(x);
      }
    }
  }
  return r2_f/=(Xmax);
}

void eq_r2_f::gradient(const double *x,double *grad){
  int    p,m;
  int    index;
  double para_z2_p[8];
  double grad_z2_p[2];

  para_z2_p[2] = dt;
  para_z2_p[5] = Freq;
  para_z2_p[6] = Xmax;
  para_z2_p[7] = LatticeSize;
  grad[0] = 0.0;
  grad[1] = 0.0;
  for(m=0;m<=mode;m++)
  {
    if(m!=0)
    {
      index = m;
      for(p=0;p<2;p++)
      {
	para_z2_p[0] = p;
	para_z2_p[1] = m;
	para_z2_p[3] = cov_z_m[p][index];
	para_z2_p[4] = mean_z_m[p][index];
	z2_p.parameterize(para_z2_p);
	z2_p.gradient(x,grad_z2_p);
	//	  cout << "[p,m,n]=" << p << ", " << m << ", " << n << "\n";
	//	  cout << "grad = [" << grad_z2_p[0] << ", " << grad_z2_p[1] << ", " << grad_z2_p[2] << "]\n";
	grad[0] += grad_z2_p[0];
	grad[1] += grad_z2_p[1];
      }
    }
  }
  grad[0] /= (Xmax);
  grad[1] /= (Xmax);
}  

bool eq_r2_f::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    mode = int(para_val);
    break;
  case 1:
    Xmax = para_val;
    z2_p.parameterize(6,Xmax);
    break;
  case 2:
    Freq = para_val;
    z2_p.parameterize(5,Freq);
    break;
  case 3:
    LatticeSize = int(para_val);
    z2_p.parameterize(7,LatticeSize);
    break;
  case 4:
    dt = para_val;
    z2_p.parameterize(2,dt);
    break;
  default:
    cerr << "er_r2_f::parameterize(): invalid PARA_ID\n";
  }
  return true;
}

bool eq_r2_f::parameterize(int para_ID,double *para_array){
  switch(para_ID){
  case 0:
    cov_z_m[0] = para_array;break;
  case 1:
    mean_z_m[0] = para_array;break;
  case 2:
    cov_z_m[1] = para_array;break;
  case 3:
    mean_z_m[1] = para_array;break;
  default:
    cerr << "ef_f::parameterize(int, double*): invalid PARA_ID\n";
  }
  return true;
}

// ---------------------------------------------------------------
eq_m2_f::eq_m2_f(double i_dt,int i_mode,double i_Xmax,
		 int i_LatticeSize, double i_Freq):
MathFunction(2,5),
dt(i_dt),mode(i_mode),Xmax(i_Xmax),
LatticeSize(i_LatticeSize),Freq(i_Freq),
z2_p(0,0,i_dt,0,0,i_Freq,i_Xmax,i_LatticeSize)
{
  int i,m,index;
  int dim = (mode+1);
  double temp;
  for(i=0;i<2;i++){
    cov_z_m[i] = NULL;
    mean_z_m[i] = NULL;
    K[i] = new double[dim];
    for(m=0;m<=mode;m++){
      temp = 4*sin(pi*m/LatticeSize)*sin(pi*m/LatticeSize)/(Xmax*Xmax*Xmax/LatticeSize/LatticeSize);
      index = m;
      switch(i)
      {
      case 0:
	if(m==0)
	{
	  K[i][index] = 0.0;
	}else
	{	
	  K[i][index] = temp;
	}
	break;
      case 1:
	if(m==0)
	{
	  K[i][index] = 0.0;
	}else
	{
	  K[i][index] = temp;
	}
	break;
      }
    }
  }
}

eq_m2_f::~eq_m2_f(){
  cout << "~eq_m_f()\n";
  int i;
  for(i=0;i<2;i++){
    delete []K[i];
  }
}

bool eq_m2_f::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
//    mode = int(para_val);
    break;
  case 1:
    Xmax = para_val;
    z2_p.parameterize(6,Xmax);
    break;
  case 2:
    Freq = para_val;
    z2_p.parameterize(4,Freq);
    break;
  case 3:
    LatticeSize = int(para_val);
    z2_p.parameterize(7,LatticeSize);
    break;
  case 4:
    dt = para_val;
    z2_p.parameterize(2,dt);
    break;
  default:
    cerr << "er_m2_f::parameterize(): invalid PARA_ID\n";
  }
  return true;
}

bool eq_m2_f::parameterize(int para_ID,double *para_array){
  switch(para_ID){
  case 0:
    cov_z_m[0] = para_array;break;
  case 1:
    mean_z_m[0] = para_array;break;
  case 2:
    cov_z_m[1] = para_array;break;
  case 3:
    mean_z_m[1] = para_array;break;
  case 4:
    cov_z_m[2] = para_array;break;
  case 5:
    mean_z_m[2] = para_array;break;
  case 6:
    cov_z_m[3] = para_array;break;
  case 7:
    mean_z_m[3] = para_array;break;
  default:
    cerr << "ef_f::parameterize(int, double*): invalid PARA_ID\n";
  }
  return true;
}

double eq_m2_f::value(const double *x){
  int    p,m;
  double m2_f = 0.0;
  int    index;
  double para_z2_p[8];

  para_z2_p[2] = dt;
  para_z2_p[5] = Freq;
  para_z2_p[6] = Xmax;
  para_z2_p[7] = LatticeSize;
  for(m=0;m<=mode;m++)
  {
    if(m!=0)
    {
      index = m;
      for(p=0;p<2;p++)
      {
	para_z2_p[0] = p;
	para_z2_p[1] = m;
	para_z2_p[3] = cov_z_m[p][index];
	para_z2_p[4] = mean_z_m[p][index];
	z2_p.parameterize(para_z2_p);
	m2_f += K[p][index]*z2_p.value(x);
      }
    }
  }
  return m2_f;
}

void eq_m2_f::gradient(const double *x,double *grad){
  int    p,m;
  int    index;
  double para_z2_p[8];
  double grad_z2_p[2];

  para_z2_p[2] = dt;
  para_z2_p[5] = Freq;
  para_z2_p[6] = Xmax;
  para_z2_p[7] = LatticeSize;
  grad[0] = 0.0;
  grad[1] = 0.0;
  for(m=0;m<=mode;m++){
    if(m!=0){
      index = m;
      for(p=0;p<2;p++){
	para_z2_p[0] = p;
	para_z2_p[1] = m;
	para_z2_p[3] = cov_z_m[p][index];
	para_z2_p[4] = mean_z_m[p][index];
	z2_p.parameterize(para_z2_p);
	z2_p.gradient(x,grad_z2_p);
	//	  cout << "[p,m,n]=" << p << ", " << m << ", " << n << "\n";
	//	  cout << "grad = [" << grad_z2_p[0] << ", " << grad_z2_p[1] << ", " << grad_z2_p[2] << "]\n";
	grad[0] += K[p][index]*grad_z2_p[0];
	grad[1] += K[p][index]*grad_z2_p[1];
      }
    }
  }
}  


// ---------------------------------------------------------------
eq_f::eq_f(double i_r2_set,double i_q_r,
	   double i_m2_set,double i_q_m,
	   double i_dt, int i_mode,int i_LatticeSize,double i_Xmax,double i_Freq):
MathFunction(2,9),
r2_set(i_r2_set),q_r(i_q_r),
m2_set(i_m2_set),q_m(i_q_m),
dt(i_dt),mode(i_mode),LatticeSize(i_LatticeSize),Xmax(i_Xmax),Freq(i_Freq),
f_r2_f(i_dt,i_mode,i_Xmax,i_LatticeSize,i_Freq),
f_m2_f(i_dt,i_mode,i_Xmax,i_LatticeSize,i_Freq)
{}
eq_f::~eq_f(){}

double eq_f::value(const double *x){
  double r2_f = f_r2_f.value(x);
  double m2_f = f_m2_f.value(x);
  return q_r*(r2_set-r2_f)*(r2_set-r2_f)/(r2_set*r2_set)+q_m*(m2_set-m2_f)*(m2_set-m2_f)/(m2_set*m2_set);
}

void eq_f::gradient(const double *x,double *grad){
  double grad_r2_f[2];
  f_r2_f.gradient(x,grad_r2_f);
  double r2_f = f_r2_f.value(x);

  double grad_m2_f[2];
  f_m2_f.gradient(x,grad_m2_f);
  double m2_f = f_m2_f.value(x);
  grad[0] = -2*q_r*(r2_set-r2_f)*grad_r2_f[0]/(r2_set*r2_set)-2*q_m*(m2_set-m2_f)*grad_m2_f[0]/(m2_set*m2_set);
  grad[1] = -2*q_r*(r2_set-r2_f)*grad_r2_f[1]/(r2_set*r2_set)-2*q_m*(m2_set-m2_f)*grad_m2_f[1]/(m2_set*m2_set);
}

bool eq_f::parameterize(int para_ID,double para_val){
  switch(para_ID)
  {
  case 0:
    r2_set = para_val;break;
  case 1:
    q_r = para_val;break;
  case 2:
    m2_set = para_val;break;
  case 3:
    q_m = para_val;break;
  case 4:
//    mode = int(para_val);
//    f_r2_f.parameterize(0,para_val);
//    f_m2_f.parameterize(0,para_val);
    break;
  case 5:
//    LatticeSize = int(para_val);
//    f_r2_f.parameterize(3,para_val);
//    f_m2_f.parameterize(3,para_val);
    break;
  case 6:
//    Xmax = para_val;
//    f_r2_f.parameterize(1,para_val);
//    f_m2_f.parameterize(1,para_val);
    break;
  case 7:
    dt = para_val;
    f_r2_f.parameterize(4,para_val);
    f_m2_f.parameterize(4,para_val);
    break;
  case 8:
    Freq = para_val;
    f_r2_f.parameterize(2,para_val);
    f_m2_f.parameterize(2,para_val);
    break;
  default:
    cerr << "eq_f::parameterize(): invalid PARA_ID\n";
  }
  return true;
}

bool eq_f::parameterize(int para_ID,double *para_array){
  f_r2_f.parameterize(para_ID,para_array);
  f_m2_f.parameterize(para_ID,para_array);
  return true;  
}



